#include "user.h"

static TaskHandle_t AppTaskCreate_Handle;                   //任务创建句柄
static TaskHandle_t USART_Read_Task_Handle;                 //串口读取读取句柄
static TaskHandle_t LCD_Refresh_Handle;                     //LCD刷新句柄
static TaskHandle_t MPU6050_Recevie_Handle;                 //MPU6050数据读取任务句柄
static TaskHandle_t Car_Move_Handle;                        //车子移动句柄
//static TaskHandle_t Sevro_Control_Handle;                   //机械臂任务
static TaskHandle_t VOFA_Send_Handle;                       //VOFA任务
static TaskHandle_t MPU6050_PID_calculation_Handle;         //角度PID解算任务
static TaskHandle_t Move_Time_Handle;                       //小车移动时间
static TaskHandle_t Laser_Ranging_Rec_Data_Handle;          //激光测距数据读取任务句柄

static QueueHandle_t USART_Read_data;                       //串口数据队列
//static QueueHandle_t MPU6050_Yaw_Handle;                  //MPU6050航线角读取


static SemaphoreHandle_t UASRT_Read_Semaphore;              //串口互斥信号量(资源管理)
static SemaphoreHandle_t MPU6050_Recevie_Semaphore;         //MPU6050数据互斥信号量
static SemaphoreHandle_t Car_Move_Semaphore;                    //时间计数
static SemaphoreHandle_t Laser_Ranging_Rec_Data_Semaphore;  //激光测距数据   
static SemaphoreHandle_t Mpu6050_PWM_Spead_Data_Semaphore; //陀螺仪修正数据信号量

//static EventGroupHandle_t Car_Move_EventGroup;         //小车移动事件组
//static EventGroupHandle_t Car_Move_EventGroup;         //小车移动判断事件组

static u8 Car_Move_Flag;                        //小车状态
static u8*USART1_Data;
float MPU6050_Data;                             //MPU6050航线角读取
float MPU_PID_Taget_Data;                        //小车陀螺仪目标值
u16 Laser_Ranging_y_Rec_Data;                     //激光测距数据(y)
u16 Laser_Ranging_x_Rec_Data;                    //激光测距数据(x)
float Mpu6050_PWM_Spead_Data;                   //Mpu6050修正数据



TimerHandle_t time1;                            //软件定时器1句柄

Tick_HZ MPU6050_Tick_HZ = 50;                   //陀螺仪帧数
Tick_HZ LCD_Tick_HZ = 70;                      //LCD刷新率
Tick_HZ USART_Tick_HZ = 100;                    //串口刷新率
Tick_HZ car_move_Tick_HZ = 50;                  //车子电机运算刷新率
Tick_HZ Sevro_Control_Tick_HZ = 50;             //舵机刷新率
Tick_HZ VOFA_Send_Tick_HZ = 50;                  //VOFA读取帧数
Tick_HZ MPU6050_PID_calculation_Tick_HZ = 50;  //MPU6050 PID解算频率
Tick_HZ Move_Time_Tick_HZ = 100;                //Move时间计数
Tick_HZ Laser_Ranging_Rec_Data_Tick_HZ = 50;   //激光测距频率

/*
    硬件初始化
*/
void BSP_Init(void)
{
 
    SystemInit();
    NVIC_PriorityGroupConfig(NVIC_PriorityGroup_2);
    delay_init(72);
    USART_1_Init(115200);
   
    UART5_Init(9600);
    LCD_Init(1);
    BSP_i2c_Init();
    Motor_Init();

    MPU_Init();
    mpu_dmp_init();
    LASER_RANGING_Init();


}

//FreeRTOS启动
 void  Task_Start(void)
{
    u16 i,t;
    i = 4000;
    t =3500;
    float xxx;
    
    while(i--)
    {
        mpu_dmp_get_data(&xxx,&xxx,&xxx);
        while(t--);
    }

    
xTaskCreate(AppTaskCreate
            ,"AppTaskCreate"
            ,512
            ,NULL
            ,8
            ,AppTaskCreate_Handle);
vTaskStartScheduler();
}

//任务创建
void AppTaskCreate(void*pv)
{
  portENTER_CRITICAL();
  //queue_1 = xQueueCreate(4,sizeof(float_p));
	/*
    time1 = xTimerCreate("timer1"
                        ,200
                        ,pdTRUE
                        ,(void *)1
                        ,(TimerCallbackFunction_t)PID_time);
    */
    UASRT_Read_Semaphore = xSemaphoreCreateMutex();
    MPU6050_Recevie_Semaphore = xSemaphoreCreateMutex();
    Car_Move_Semaphore = xSemaphoreCreateMutex();
    Laser_Ranging_Rec_Data_Semaphore = xSemaphoreCreateMutex();
    Mpu6050_PWM_Spead_Data_Semaphore = xSemaphoreCreateMutex();
    
    USART_Read_data = xQueueCreate(1,sizeof(u8));
    //MPU6050_Yaw_Handle = xQueueCreate(1,sizeof(float));
    
	
    xTaskCreate(USART_Read_Task
                ,"USART_Read_Task"
                ,128
                ,&USART_Tick_HZ
                ,6
                ,USART_Read_Task_Handle);
    
    
    xTaskCreate(LCD_Refresh_Task
                ,"LCD_Refresh_Task"
                ,512
                ,&LCD_Tick_HZ
                ,3
                ,LCD_Refresh_Handle);  
    
           
    xTaskCreate(MPU6050_Receive_Task
                ,"MPU6050_Receive_Task"
                ,256
                ,&MPU6050_Tick_HZ
                ,5
                ,MPU6050_Recevie_Handle);
    
    /*
    xTaskCreate(Car_Move_Task
                ,"Car_Move_Task"
                ,1023
                ,&car_move_Tick_HZ
                ,2
                ,Car_Move_Handle);
    */
    
    xTaskCreate(Move_Time_Task
                ,"Move_Time_Task"
                ,1023
                ,&Move_Time_Tick_HZ
                ,6
                ,Move_Time_Handle);
    
    
    xTaskCreate(Laser_Ranging_Rec_Data_Task
                ,"Laser_Ranging_Rec_Data_Task"
                ,512
                ,&Laser_Ranging_Rec_Data_Tick_HZ
                ,2
                ,Laser_Ranging_Rec_Data_Handle);
    
    
    
    xTaskCreate(MPU6050_PID_calculation_Task
                ,"MPU6050_PID_calculation_Task"
                ,255
                ,&MPU6050_PID_calculation_Tick_HZ
                ,2
                ,MPU6050_PID_calculation_Handle);

    
    

    xTaskCreate(VOFA_Send_Task
                ,"VOFA_Send_Task"
                ,128
                ,&VOFA_Send_Tick_HZ
                ,2
                ,VOFA_Send_Handle);
    
    vTaskDelete(AppTaskCreate_Handle);
    
    portEXIT_CRITICAL();
}

//串口读取数据
void USART_Read_Task(void*pv)
{
    static 
     u8 USART_Data;
    u8 MaxWait = *(u8*)pv / portTICK_RATE_MS;
    TickType_t time= xTaskGetTickCount();
    while(1)
    {   

        if(xSemaphoreTake(UASRT_Read_Semaphore,0) == pdTRUE)
        {        
            USART1_Data = USART1_ReadPackage();
            xQueueSend(USART_Read_data,&USART_Data,11);
            xSemaphoreGive(UASRT_Read_Semaphore);
        }

        vTaskDelayUntil(&time,MaxWait);
    }
}

//显示屏刷新程序
void LCD_Refresh_Task(void*pv)
{
    TickType_t time= xTaskGetTickCount();
    //static u8 USART_Data;
    u8 MaxWait = *(u8*)pv / portTICK_RATE_MS;
    static u8 i=0;
    static u8*data;
    static float MPU_read_data_yaw;
    static u8 car_flag;
    static u16 LR_y_Data;
    static u16 LR_x_Data;
    static u8*usart1;
    while(1)
    {
         if(xSemaphoreTake(Car_Move_Semaphore,1) == pdTRUE)
            {
			    car_flag = Car_Move_Flag;
            xSemaphoreGive(Car_Move_Semaphore);
            }
        data = USART5_ReadPackage();
        LCD_ShowNum(0,0,i,3,16);
        i++;
        

        if(xSemaphoreTake(Laser_Ranging_Rec_Data_Semaphore,1) == pdTRUE)
        {
			LR_y_Data = Laser_Ranging_y_Rec_Data;
            LR_x_Data = Laser_Ranging_x_Rec_Data;
            xSemaphoreGive(Laser_Ranging_Rec_Data_Semaphore);
        }

        if(xSemaphoreTake(UASRT_Read_Semaphore,0) == pdTRUE)
        {        
            USART1_Data = usart1;
            xSemaphoreGive(UASRT_Read_Semaphore);
        }
        LCD_ShowNum(59,59,LR_y_Data,4,16);
        LCD_ShowNum(0,59,LR_x_Data,4,16);

	    //USART_Data = USART5_ReadData();
	    //xQueueReceive(USART_Read_data,&USART_Data,11);


		//xSemaphoreTake(MPU6050_Recevie_Semaphore,1);
       	//LCD_ShowNum(0,16,MPU6050_Data,4,16);
		//xSemaphoreGive(MPU6050_Recevie_Semaphore);

        //LCD_ShowNum(0,59,usart1[2],3,16);
		LCD_ShowNum(0,16,Read_Speed(0),3,16);
        LCD_ShowNum(27,16,Read_Speed(1),3,16);
        LCD_ShowNum(54,16,Read_Speed(2),3,16);
        LCD_ShowNum(81,16,Read_Speed(3),3,16);

        LCD_ShowNum(0,32,Motor_PID_Out(0,F_MOVE),4,16);
        LCD_ShowNum(27,32,Motor_PID_Out(1,F_MOVE),4,16);
        LCD_ShowNum(54,32,Motor_PID_Out(2,F_MOVE),4,16);
        LCD_ShowNum(81,32,Motor_PID_Out(3,F_MOVE),4,16);
        //LCD_ShowNum(0,48,car_flag,4,16);
        LCD_ShowNum(0,96,data[1],3,16);




        vTaskDelayUntil(&time,MaxWait);

        if(xSemaphoreTake(MPU6050_Recevie_Semaphore,1) == pdTRUE)
        {
			MPU_read_data_yaw = MPU6050_Data;
            xSemaphoreGive(MPU6050_Recevie_Semaphore);
        }
        if(MPU_read_data_yaw>=0)
        {
            LCD_ShowNum(0,112,MPU_read_data_yaw,4,16);
        }
        else
        {
            LCD_ShowChar(0,112,BLACK,WHITE,'-',16,4);
            LCD_ShowNum(17,112,MPU_read_data_yaw,4,16);
        }
        
        //USART5_PrintDatas("LCD_Refresh_Task\n");
        vTaskDelayUntil(&time,MaxWait);
    }
}

//MPU6050数据读取任务
void MPU6050_Receive_Task(void*pv)
{
	static float MPU_read_data_yaw,z,x,MPU6050_xiu;
	//u8 i = 0,k=0;
    static u8 Temp = 0;
     u8 MaxWait = *(u8*)pv / portTICK_RATE_MS;
    static u8 status;
    TickType_t time= xTaskGetTickCount();
    while(1)
    {
        if(status == 0)
        {
            mpu_dmp_get_data(&x,&z,&MPU6050_xiu);
            status = 1;
        }
		
		mpu_dmp_get_data(&x,&z,&MPU_read_data_yaw);
			
        if(xSemaphoreTake(MPU6050_Recevie_Semaphore,1) == pdTRUE)
        {
			MPU6050_Data = MPU_read_data_yaw-MPU6050_xiu ;
            xSemaphoreGive(MPU6050_Recevie_Semaphore);
        }
		Temp++;
        if(Temp == 100)
        {
            MPU6050_xiu = MPU6050_xiu-1.5;
            Temp = 0;
        }
        vTaskDelayUntil(&time,MaxWait);
    }
}


//小车移动任务
void Car_Move_Task(void*pv)
{
    TickType_t time= xTaskGetTickCount();
    u8 MaxWait = *(u8*)pv / portTICK_RATE_MS;
    static int Target[4];
    static u8 Move_Flag;
    static u8 Temp;
    static float MPU6050_out_data; 

    //static u8*data;

    while(1)
    {
        if(xSemaphoreTake(Mpu6050_PWM_Spead_Data_Semaphore,1) == pdTRUE)
        {
			MPU6050_out_data = Mpu6050_PWM_Spead_Data;
            xSemaphoreGive(Mpu6050_PWM_Spead_Data_Semaphore);
        }
        if(xSemaphoreTake(Car_Move_Semaphore,1) == pdTRUE)
        {
			Move_Flag = Car_Move_Flag;
            xSemaphoreGive(Car_Move_Semaphore);
        }

        if(Move_Flag == CAR_MOVE_F)
        {
            Target[0] = (20+MPU6050_out_data);
            Target[1] = (20-MPU6050_out_data);
            Target[2] = (20-MPU6050_out_data);
            Target[3] = (20+MPU6050_out_data);
            for(Temp = 0;Temp<4;Temp++)
                Motor_PID_Move(Temp,Target[Temp]);
        }
        else if(Move_Flag == CAR_MOVE_B)
        {

            Target[0] = -(20-MPU6050_out_data);
            Target[1] = -(20+MPU6050_out_data);
            Target[2] = -(20+MPU6050_out_data);
            Target[3] = -(20-MPU6050_out_data);
            for(Temp = 0;Temp<4;Temp++)
                Motor_PID_Move(Temp,Target[Temp]);

        }
        else if(Move_Flag == CAR_MOVE_R)
        {
            
            Target[0] = -(20-MPU6050_out_data); //-
            Target[1] = +(20-MPU6050_out_data); //+
            Target[2] = -(20+MPU6050_out_data); //-
            Target[3] = +(20+MPU6050_out_data); //+
            

            for(Temp = 0;Temp<4;Temp++)
                Motor_PID_Move(Temp,Target[Temp]);
        }
        else if(Move_Flag == CAR_MOVE_L)
        {
            
            Target[0] = +(20+MPU6050_out_data); //-
            Target[1] = -(20+MPU6050_out_data); //+
            Target[2] = +(20-MPU6050_out_data); //-
            Target[3] = -(20-MPU6050_out_data); //+
            

            for(Temp = 0;Temp<4;Temp++)
                Motor_PID_Move(Temp,Target[Temp]);

        }
        else if(Move_Flag == CAR_MOVE_P)
        {
            Target[0] = 0;
            Target[1] = 0;
            Target[2] = 0;
            Target[3] = 0;
             for(Temp = 0;Temp<4;Temp++)
                Motor_PID_Move(Temp,Target[Temp]);
        }

        else if(Move_Flag == CAR_REVOLVE_CW)
        {
            Target[0] = -20;
            Target[1] = 20;
            Target[2] = 20;
            Target[3] = -20;
             for(Temp = 0;Temp<4;Temp++)
                Motor_PID_Move(Temp,Target[Temp]);
        }
        else if(Move_Flag == CAR_REVOLVE_CCW)
        {
            Target[0] = 20;
            Target[1] = -20;
            Target[2] = -20;
            Target[3] = 20;
             for(Temp = 0;Temp<4;Temp++)
                Motor_PID_Move(Temp,Target[Temp]);
        }
        vTaskDelayUntil(&time,MaxWait);
    
       
    }
}

//VOFA发送任务
void VOFA_Send_Task(void*pv)
{
    TickType_t time= xTaskGetTickCount();
    u8 MaxWait = *(u8*)pv / portTICK_RATE_MS;
    static float MPU_read_data_yaw;
    while(1)
    {
        if(xSemaphoreTake(MPU6050_Recevie_Semaphore,1) == pdTRUE)
        {
			MPU_read_data_yaw = MPU6050_Data;
            xSemaphoreGive(MPU6050_Recevie_Semaphore);
        }
        Vofa_Input(MPU_read_data_yaw,0);
        Vofa_Send();
        vTaskDelayUntil(&time,MaxWait);
    }
}

//小车移动判断任务
void Move_Time_Task(void*pv)
{
    TickType_t time= xTaskGetTickCount();
    u8 MaxWait = *(u8*)pv / portTICK_RATE_MS;
    static u16 Use_Time;
    static u16 yd,xd;
    static u8 Car_stat = 1;
    static Car_Task_stat Car_Task_data;
    Car_Task_data.Car_Move_flag =  CAR_MOVE_P;
    static u8*data_usart1;


    
    while(1)
    {
       
        if(xSemaphoreTake(Laser_Ranging_Rec_Data_Semaphore,1) == pdTRUE)
        {
		    xd =  Laser_Ranging_x_Rec_Data;
		    yd =  Laser_Ranging_y_Rec_Data;

            xSemaphoreGive(Laser_Ranging_Rec_Data_Semaphore);
        }
        if(xSemaphoreTake(UASRT_Read_Semaphore,0) == pdTRUE)
        {        
            data_usart1 = USART1_Data;
            xSemaphoreGive(UASRT_Read_Semaphore);
        }


        Car_Task_data.Laser_Data[0] = xd;
        Car_Task_data.Laser_Data[1] = yd;

        Car_Task_data.usart1_data[0] = data_usart1[1];
        Car_Task_data.usart1_data[1] = data_usart1[2];
        Car_Task_data.usart1_data[2] = data_usart1[3];
       

        Car_Task_Judge(&Car_Task_data);

        if(xSemaphoreTake(Car_Move_Semaphore,1) == pdTRUE)
        {
			Car_Move_Flag =  Car_Task_data.Car_Move_flag;
            
            xSemaphoreGive(Car_Move_Semaphore);
        }
        if(xSemaphoreTake(MPU6050_Recevie_Semaphore,1) == pdTRUE)
        {
			MPU_PID_Taget_Data = Car_Task_data.MPU_PID_Taget;
            Car_Task_data.MPU_Read_Data = MPU6050_Data; 
            xSemaphoreGive(MPU6050_Recevie_Semaphore);
        }
       
        

        vTaskDelayUntil(&time,MaxWait);
    }
}

//激光数据读取任务
void Laser_Ranging_Rec_Data_Task(void*pv)
{
    TickType_t time= xTaskGetTickCount();
    u8 MaxWait = *(u8*)pv / portTICK_RATE_MS;
    static u8 Temp;

    while(1)
    {

        for(Temp = 0;Temp<10;Temp++)
        {
            LASER_RANGING_Send_instructions();


            if(xSemaphoreTake(Laser_Ranging_Rec_Data_Semaphore,1) == pdTRUE)
            {
			    Laser_Ranging_x_Rec_Data =  distance_x_Read();
			    Laser_Ranging_y_Rec_Data =  distance_y_Read();

                xSemaphoreGive(Laser_Ranging_Rec_Data_Semaphore);
            }
        }
        vTaskDelayUntil(&time,MaxWait);
        
    }
}


//MPU6050航向角PID矫正
void MPU6050_PID_calculation_Task(void*pv)
{
    static PID_Handle MPU6050_pid;
    TickType_t time= xTaskGetTickCount();
    u8 MaxWait = *(u8*)pv / portTICK_RATE_MS;
    MPU6050_pid.P = 2;
    MPU6050_pid.I = 0.05;
    MPU6050_pid.D = 0;
    MPU6050_pid.OutputMax = 10;
    MPU6050_pid.OutputMin = -10;
    
    MPU6050_pid.out_zoom = 1;
    static float MPU_read_data_yaw;

    while(1)
    {
        if(xSemaphoreTake(MPU6050_Recevie_Semaphore,1) == pdTRUE)
        {
			MPU_read_data_yaw = MPU6050_Data;
            MPU6050_pid.Target  = MPU_PID_Taget_Data;
            xSemaphoreGive(MPU6050_Recevie_Semaphore);
        }

        PID_IncOperation(&MPU6050_pid,MPU_read_data_yaw);

         if(xSemaphoreTake(Mpu6050_PWM_Spead_Data_Semaphore,1) == pdTRUE)
        {
			Mpu6050_PWM_Spead_Data = MPU6050_pid.Output;
            xSemaphoreGive(Mpu6050_PWM_Spead_Data_Semaphore);
        }
        vTaskDelayUntil(&time,MaxWait);
    }
}


